#include "Heightfield.h"
#include "BoundingBox.h"
#include "HitRecord.h"
#include "Point.h"
#include "Ray.h"
#include "Vector.h"
#include <math.h>

Heightfield::Heightfield(Material* material, const Point& corner1,
		const Point& corner2, std::string myfile) :
	Primitive(material) {
	bbox.extend(corner1);
	bbox.extend(corner2);
	file = myfile;
	initialize(file);
}

Heightfield::~Heightfield() {
}

void Heightfield::initialize(std::string filename) {
	std::ifstream in(filename.c_str(), ios::in | ios::binary);
	if (!in) {
		std::cerr << "Error opening "<< filename
				<< " for initialization of Heightfield\n";
		exit(1);
	}
	double minz, maxz;
	in >> nx >> ny >> minz >> maxz;
	if (!in) {
		std::cerr << "Error reading header from "<< filename
				<< " for initialization of Heightfield\n";
		exit(1);
	}
	std::cout << "Reading "<< filename << " shows "<< nx << " by "<< ny
			<< " min "<< minz << " and max "<< maxz << std::endl;
	in.get();
	data = new float*[nx+1];
	float* p = new float[(nx+1)*(ny+1)];
	for (int i=0; i<=nx; i++)
		data[i] = p+i*(ny+1);
	in.read(reinterpret_cast<char*>(data[0]), sizeof(float)*(nx+1)*(ny+1));
	//for (int i = 0; i <= nx; i++)
	//	for (int j = 0; j <= ny; j++)
	//		in >> data[i][j];
	if (!in) {
		std::cerr << "Error reading data from "<< filename << "\n";
		if(in.fail())
			std::cerr << "filestream 'fail' is true\n";
		if(in.bad())
			std::cerr << "filestream 'bad' is true\n";
		if(in.eof())
			std::cerr << "filestream 'eof' is true\n";
				
		exit(1);
	}

	double dz = maxz-minz;
	if (dz < 1.e-3)
		dz = 1.e-3;

	Point old_min = bbox.getMin();
	Point old_max = bbox.getMax();
	Point new_min(old_min.x(), old_min.y(), minz-dz*1.e-4);
	Point new_max(old_max.x(), old_max.y(), maxz+dz*1.e-4);
	bbox.set_corners(new_min, new_max);
	//min = Point(min.x, min.y, minz-dz*1.e-4);
	//max = Point(max.x, max.y, maxz+dz*1.e-4);

	// Step 1
	diag = new_max-new_min;
	cellsize = diag/Vector(nx, ny, 1);
	inv_cellsize = cellsize.inverse();

}

void Heightfield::getBounds(BoundingBox& ret_box) const {
	ret_box.set_corners(bbox.getMin(), bbox.getMax());
}

void Heightfield::intersect(HitRecord& hit, const RenderContext& context,
		const Ray& ray) const {
	//Procedure: For each cell, check if the ray intersects the bilinear patch
	//if it doesn't, move the ray's origin to the border between this cell
	//and the next. Do this until the ray either hits a patch or leaves the box

	//bbox.intersect(hit, context, ray);

	if (hit.minT() < DBL_MAX) {
		Point minp = bbox.getMin();
		Point maxp = bbox.getMax();
		// Step 2
		Vector idir = ray.direction().inverse();
		Vector v1 = (minp - ray.origin()) * idir;
		Vector v2 = (maxp - ray.origin()) * idir;
		Vector vmin = Min(v1, v2);
		Vector vmax = Max(v1, v2);
		double tnear = vmin.maxComponent();
		double tfar = vmax.minComponent();
		if (tnear >= tfar)
			return;
		if (tfar < 1.e-6)
			return;
		if (tnear < 0)
			tnear = 0;

		// Step 3
		Point p = ray.origin() + ray.direction() * tnear;
		int Lx = (int)( (p.x() - minp.x() ) * inv_cellsize.x() );
		Lx = Lx < 0 ? 0 : Lx >= nx ? nx - 1 : Lx;
		int Ly = (int)( (p.y() -minp.y() ) * inv_cellsize.y() );
		if (Ly < 0)
			Ly = 0;
		else if (Ly >= ny)
			Ly = ny-1;

		// Step 4
		double signx = diag.x() * ray.direction().x();
		double signy = diag.y() * ray.direction().y();
		int dix = signx>0 ? 1 : -1;
		int stopx = signx>0 ? nx : -1;
		int diy = signy>0 ? 1 : -1;
		int stopy = signy>0 ? ny : -1;

		// Step 5
		double dtdx = abs(cellsize.x() / ray.direction().x() );
		double dtdy = abs(cellsize.y() / ray.direction().y() );

		// Step 6
		double far_x;
		if (signx>0)
			far_x = (Lx + 1) * cellsize.x() + minp.x();
		else
			far_x = Lx * cellsize.x() + minp.x();
		double far_y;
		if (signy>0)
			far_y = (Ly + 1) * cellsize.y() + minp.y();
		else
			far_y = Ly * cellsize.y() + minp.y();

		// Step 7
		double tnext_x = (far_x - ray.origin().x() ) / ray.direction().x();
		double tnext_y = (far_y - ray.origin().y() ) / ray.direction().y();

		// Step 8
		float zenter = ray.origin().z() + tnear * ray.direction().z();
		tfar = min(tfar, hit.minT());
		while (tnear < tfar) {
			double texit = min(tnext_x, tnext_y);
			float zexit = ray.origin().z() + texit * ray.direction().z();

			// Step 9
			float datamin = Min(data[Lx][Ly], data[Lx+1][Ly], data[Lx][Ly+1],
					data[Lx+1][Ly+1]);
			float datamax = Max(data[Lx][Ly], data[Lx+1][Ly], data[Lx][Ly+1],
					data[Lx+1][Ly+1]);
			float zmin = min(zenter, zexit);
			float zmax = max(zenter, zexit);
			if (zmin < datamax && zmax > datamin) {
				// Step 10
				Point C = minp + Vector(Lx, Ly, 0)*cellsize;
				Vector EC = ray.origin() + ray.direction() * tnear - C;
				double Ex = EC.x() * inv_cellsize.x();
				double Ey = EC.y() * inv_cellsize.y();
				double Ez = ray.origin().z() + tnear * ray.direction().z();
				double Vx = ray.direction().x() * inv_cellsize.x();
				double Vy = ray.direction().y() * inv_cellsize.y();
				double Vz = ray.direction().z();
				float za = data[Lx][Ly];
				float zb = data[Lx+1][Ly]-za;
				float zc = data[Lx][Ly+1]-za;
				float zd = data[Lx+1][Ly+1]-zb-zc-za;
				double a = Vx*Vy*zd;
				double b = -Vz + Vx*zb + Vy*zc + (Ex*Vy + Ey*Vx)*zd;
				double c = -Ez + za + Ex*zb + Ey*zc + Ex*Ey*zd;
				if (abs(a) < 1.e-6) {
					// Linear
					double tcell = -c/b;
					if (tcell > 0&& tnear+tcell < texit) {
						double tsum = tnear+tcell;
						if (hit.hit(tsum, this, matl) ) {
							HitData* hd = hit.getScratchpad<HitData>();
							hd->x = Lx;
							hd->y = Ly;
							return;
						}
					}
				} else {
					// Solve quadatric
					double disc = b*b-4*a*c;
					if (disc > 0) {
						double root = sqrt(disc);
						double tcell1 = (-b + root)/(2*a);
						double tcell2 = (-b - root)/(2*a);
						if (tcell1 >= 0&& tnear+tcell1 <= texit) {
							double tsum = tnear+tcell1;
							if (hit.hit(tsum, this, matl)) {
								if (tcell2 >= 0) {
									// Check the other root in case it is closer.
									// No need for an additional if() because it will still
									// be in the same cell
									if (hit.hit(tnear+tcell2, this, matl)) {
										HitData* hd = hit.getScratchpad<HitData>();
										hd->x = Lx;
										hd->y = Ly;
										return;
									}
								} 
								return;
							}
						} else if (tcell2 >= 0&& tnear+tcell2 <= texit) {
							double tsum = tnear + tcell2;
							if (hit.hit(tsum, this, matl) ) {
								HitData* hd = hit.getScratchpad<HitData>();
								hd->x = Lx;
								hd->y = Ly;
								return;
							}
						}
					}
				}
			}
			// Step 11
			zenter = zexit;
			if (tnext_x < tnext_y) {
				Lx += dix;
				if (Lx == stopx)
					break;
				tnear = tnext_x;
				tnext_x += dtdx;
			} else {
				Ly += diy;
				if (Ly == stopy)
					break;
				tnear = tnext_y;
				tnext_y += dtdy;
			}
		}
	}

	return;
}

void Heightfield::normal(Vector& normal, const RenderContext&,
		const Point& hitpos, const Ray& ray, const HitRecord& hit) const {
	//const HitData* hd = hit.getScratchpad<HitData>();
	//int Lx = hd->x;
	//int Ly = hd->y;
	Point minp = bbox.getMin();
	int Lx = (int)( ( hitpos.x() - minp.x() ) * inv_cellsize.x() );
	//Lx = Lx < 0 ? 0 : Lx >= nx ? nx-1 : Lx;
	if( Lx < 0 )
		Lx = 0;
	else if(Lx >= nx)
		Lx = nx - 1;
	
		
	int Ly = (int)( ( hitpos.y() - minp.y() ) * inv_cellsize.y() );

	Point C = minp + Vector(Lx, Ly, 0) * cellsize;
	double u = (hitpos.x() - C.x() ) * inv_cellsize.x();
	double v = (hitpos.y() - C.y() ) * inv_cellsize.y();
	double dudx = inv_cellsize.x();
	double dvdy = inv_cellsize.y();
	float za = data[Lx][Ly];
	float zb = data[Lx+1][Ly]-za;
	float zc = data[Lx][Ly+1]-za;
	float zd = data[Lx+1][Ly+1]-zb-zc-za;
	double px = dudx*zb + dudx*v*zd;
	double py = dvdy*zc + dvdy*u*zd;
	normal = Vector(-px, -py, 1);
	normal.normalize();
	return;

}
